' Mini_Sumo_3.3_Simple_Mini_Sumo.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}

'-----[ I/O Definitions ]------------------------------------------------

LMotor         PIN      13                ' left servo motor
RMotor         PIN      12                ' right servo motor

LLinePwr       PIN      10                ' left line sensor power
LLineIn        PIN       9                ' left line sensor input
RLinePwr       PIN       7                ' right line sensor power
RLineIn        PIN       8                ' right line sensor input

StartLED       PIN       0                ' display start delay

'-----[ Constants ]------------------------------------------------------

LFwdFast       CON      1000              ' left motor fwd; fast
LFwdSlow       CON       800              ' left motor fwd; slow
LStop          CON       750              ' left motor stop
LRevSlow       CON       700              ' left motor rev; slow
LRevFast       CON       500              ' left motor rev; fast

RFwdFast        CON      500              ' right motor fwd; fast
RFwdSlow        CON      700              ' right motor fwd; slow
RStop           CON      750              ' right motor stop
RRevSlow        CON      800              ' right motor rev; slow
RRevFast        CON     1000              ' right motor rev; fast

'-----[ Variables ]------------------------------------------------------

lLine           VAR     Word              ' left sensor raw reading
rLine           VAR     Word              ' right sensor raw reading
lineBits        VAR     Nib               ' decoded sensors value
lbLeft          VAR     lineBits.BIT1
lbRight         VAR     lineBits.BIT0

pulses          VAR     Byte              ' counter for motor control
temp            VAR     Byte

'-----[ EEPROM Data ]----------------------------------------------------

RunStatus       DATA    $00               ' run status

'-----[ Initialization ]-------------------------------------------------

Reset:
  READ RunStatus, temp                    ' read current status
  temp = ~temp                            ' invert status
  WRITE RunStatus, temp                   ' save for next reset
  IF (temp > 0) THEN END                  ' run now?

Start_Delay:
  HIGH StartLED                           ' show active
  PAUSE 5000                              ' start delay
  LOW StartLED                            ' LED off

'-----[ Program Code ]---------------------------------------------------

Main:
  GOSUB Read_Line_Sensors

  ' sumo movement
  BRANCH lineBits, [Go_Fwd, Spin_Left, Spin_Right, About_Face]

Go_Fwd:
  PULSOUT LMotor, LFwdSlow
  PULSOUT RMotor, RFwdSlow
  GOTO Main

Spin_Left:
  FOR pulses = 1 TO 20
    PULSOUT LMotor, LRevSlow
    PULSOUT RMotor, RFwdSlow
    PAUSE 20
  NEXT
  GOTO Main

Spin_Right:
  FOR pulses = 1 TO 20
    PULSOUT LMotor, LFwdSlow
    PULSOUT RMotor, RRevSlow
    PAUSE 20
  NEXT
  GOTO Main

About_Face:
  FOR pulses = 1 TO 10                    ' back up from edge
    PULSOUT LMotor, LRevSlow
    PULSOUT RMotor, RRevSlow
    PAUSE 20
  NEXT
  FOR pulses = 1 TO 30                    ' turn around
    PULSOUT LMotor, LFwdSlow
    PULSOUT RMotor, RRevSlow
    PAUSE 20
  NEXT
  GOTO Main

  END

'-----[ Subroutines ]----------------------------------------------------

Read_Line_Sensors:
  HIGH LLinePwr                           ' activate sensors
  HIGH RLinePwr
  HIGH LLineIn                            ' discharge caps
  HIGH RLineIn
  PAUSE 1
  RCTIME LLineIn, 1, lLine                ' read left sensor
  RCTIME RLineIn, 1, rLine                ' read right sensor
  LOW LLinePwr                            ' deactivate sensors
  LOW RLinePwr

  ' convert readings to bits
  LOOKDOWN lLine, >=[10, 0], lbLeft     ' 0 = black, 1 = line
  LOOKDOWN rLine, >=[10, 0], lbRight
  RETURN